#include <stdlib.h>
#include "avr_compiler.h"
#include "uart.h"
#include <stdio.h>


/*Command Lengths*/
#define FOUR_BYTE 4
#define SIX_BYTE 3

/* Command Defines */
#define GET_EVENT_STATUS 0x31
#define GET_ANALOG 0x28
#define GET_MOTOR_COMMAND 0x69
#define GET_MOTOR_MODE 0xDD
#define SET_MOTOR_COMMAND 0x77
#define SET_MOTOR_MODE 0xDC
#define PMD_RESET 0x39
#define GET_MOTOR_LIMIT 0x07
#define SET_MOTOR_LIMIT 0x06
#define SET_PWM_FREQUENCY 0x0C
#define GET_PWM_FREQUENCY 0x0D
#define GET_COMMUTATION_MODE 0xE3
#define SET_COMMUTATION_MODE 0xE2
#define SET_PHASE_COUNTS 0x75
#define GET_PHASE_COUNTS 0x7D
#define SET_DEAD_TIME 0x16
#define GET_DEAD_TIME 0x17
#define GET_PWM_LIMIT 0x21
#define SET_PWM_LIMIT 0x20
#define GET_ACTIVITY_STATUS 0xA6
#define SET_CONDITION_MASK 0x63
#define SET_SIGNAL_SENSE 0xA2
#define SET_PWM_SENSE 0x73

//For Serial Port Setup 4608 baud
#define SERIAL_PORT_4608 0x0007
#define SET_SERIAL_PORT_MODE 0x8B


#define SET_LOOP_MODE 0x6F
#define GET_LOOP_MODE 0x70
#define SET_VELOCITY_FEEDBACK_SOURCE 0x4E
#define GET_VELOCITY_FEEDBACK_SOURCE 0x4F
#define SET_VELOCITY_SCALAR 0x23
#define GET_VELOCITY_SCALAR 0x24
#define SET_VELOCITY 0x11
#define GET_VELOCITY 0x4B
#define SET_SAMPLE_TIME 0x38
#define GET_SAMPLE_TIME 0x61
#define SET_LOOP_GAIN 0x78
#define GET_LOOP_GAIN 0x79
#define GET_LOOP_ERROR 0x2E
#define GET_LOOP_INTEGRAL 0x3D
#define SET_MOTION_ERROR_LIMIT 0xCC
#define GET_MOTION_ERROR_LIMIT 0xCD
#define SET_PHASE_PRESCALE 0xE6
#define GET_PHASE_PRESCALE 0xE7
#define SET_PHASE_CORRECTION_MODE 0xE8
#define GET_PHASE_CORRECTION_MODE 0xE9
#define SET_PHASE_ANGLE 0x84
#define GET_PHASE_ANGLE 0x2C
#define STORE_USER_DATA 0x71
#define SET_COMMAND_SOURCE 0x7E
#define GET_COMMAND_SOURCE 0x7F
#define SET_BUS_VOLTAGE_LIMITS 0x62
#define GET_BUS_VOLTAGE_LIMITS 0x60
#define SET_ANALOG_OFFSET 0x29
#define GET_ANALOG_OFFSET 0x2A
#define SET_ACTUAL_POSITION 0x4D
#define GET_ACTUAL_POSITION 0x37
#define SET_ACCELERATION 0x90
#define GET_ACCELERATION 0x4C
#define RESET_EVENT_STATUS 0x34
#define GET_SIGNAL_STATUS 0xA4
#define CLEAR_POSITION_ERROR 0x47
#define GET_BUS_VOLTAGE 0x40
#define GET_LOOP_ERROR 0x2E

/* Loop Mode Options */
#define LOOP_MODE_CURRENT	1				// Current Loop
#define LOOP_MODE_VELOCITY	2				// Velocity Loop
#define LOOP_MODE_VELOCITY_INTEGRATOR 4		// Velocity Integrator Loop

/* Velocity Feedback Source Options */
#define VELOCITY_FEEDBACK_SOURCE_ENCODER 0	// Quadrature Encoder 
#define VELOCITY_FEEDBACK_SOURCE_TACH	 1	// Analog Tachometer
#define VELOCITY_FEEDBACK_SOURCE_HALL	 2	// Hall Phase Sensors

/* Loop Gain Instance Options */
#define LOOP_CURRENT_KP			0x00
#define LOOP_CURRENT_KI			0x10
#define LOOP_CURRENT_LIMIT		0x20
#define LOOP_VELOCITY_KP		0x01
#define LOOP_VELOCITY_KI		0x11
#define LOOP_VELOCITY_LIMIT		0x21
#define LOOP_VELOCITY_INT_KP	0x02
#define LOOP_VELOCITY_INT_KI	0x12
#define LOOP_VELOCITY_INT_LIMIT	0x22
#define LOOP_VELOCITY_INT_KD	0x32

#define CURRENT_A 2
#define CURRENT_B 3

/* Command Source Options */
#define CMD_SOURCE_ANALOG		0 // Analog Input Source
#define CMD_SOURCE_SPI			1 // SPI Input Source
#define CMD_SOURCE_PROFILE_GEN	2 // Profile Generator Input Source

/* Bus OVLO/UVLO Options */
#define BUS_OV_LIM 0	// Bus Overvoltage Limit
#define BUS_UV_LIM 1	// Bus Undervoltage Limit

/* Autostop Modes */ 
#define AUTOSTOP_DISABLE 0 // Autostop disabled
#define AUTOSTOP_ENABLE	 1 // Autostop enabled

/* Analog Offset PortID Options */
#define ANALOG_OFFSET_ACMD 0
#define ANALOG_OFFSET_TACH 1
#define ANALOG_OFFSET_CURRENT_A
#define ANALOG_OFFSET_CURRENT_B

/* Reset Event Statuses */
#define REVENT_CAPTURE_RECEIVED_bm 0x0008
#define REVENT_MOTION_ERROR_bm	   0x0010
#define REVENT_AMPLIFIER_ERROR_bm  0x0100
#define REVENT_USER_CMD_ERROR_bm   0x8000

/* Signal Status Bit Values and Bitmasks */
#define STATUS_QUAD_A_bv 0
#define STATUS_QUAD_A_bm (1<<STATUS_QUAD_A_bv)
#define STATUS_QUAD_B_bv 1
#define STATUS_QUAD_B_bm (1<<STATUS_QUAD_B_bv)
#define STATUS_INDEX_bv 2
#define STATUS_INDEX_bm (1<<STATUS_INDEX_bv)
#define STATUS_HALL1_bv 7
#define STATUS_HALL1_bm (1<<STATUS_HALL1_bv)
#define STATUS_HALL2_bv 8
#define STATUS_HALL2_bm (1<<STATUS_HALL2_bv)
#define STATUS_HALL3_bv 9
#define STATUS_HALL_3_bm (1<<STATUS_HALL3_bv)
#define STATUS_ESTOP_bv 13
#define STATUS_ESTOP_bm (1<<STATUS_ESTOP_bv)
#define STATUS_PWM_DISABLE_bv 14
#define STATUS_PWM_DISABE_bm (1<<STATUS_PWM_DISABLE_bv)
#define STATUS_AMP_DISABLE_bv 15
#define STATUS_AMP_DISABLE_bm (1<<STATUS_AMP_DISABLE_bv)

/* Loop Integral and Error LoopID Values */
#define LOOP_ID_VELOCITY_INT 0
#define LOOP_ID_VELOCITY 1
#define LOOP_ID_CURRENT_A_D 2
#define LOOP_ID_CURRENT_B_Q 3

/* Host I/O Error Codes */
#define HOST_IO_NO_ERROR 0
#define HOST_IO_73110_REST 1
#define HOST_IO_INVALID_INSTRUCTION 3
#define HOST_IO_INVALID_PARAMETER 4
#define HOST_IO_BAD_CHECKSUM 9

/* Phase Correction Mode Options */
#define PHASE_CORRECTION_INDEX 1
#define PHASE_CORRECTION_HALL 2

typedef struct
{
	//UART module pmd is located on
	USART_data_t uart;
	volatile uint8_t data_in[20];
	volatile uint8_t command;
	volatile uint8_t data_in_count;
	volatile uint8_t data_available;
	volatile uint8_t error;
	volatile uint16_t bus_voltage;

} PMD_Control_t;

void set_serial_port_mode(PMD_Control_t * pmd, uint16_t value);
void pmd_no_operation(PMD_Control_t * pmd);
void set_pwm_sense(PMD_Control_t * pmd, uint8_t value);
void set_motor_limit(PMD_Control_t * pmd, int16_t value);
void set_pwm_frequency(PMD_Control_t * pmd, int16_t value);
void get_motor_limit(PMD_Control_t * pmd);
void get_pwm_frequency(PMD_Control_t * pmd);
void set_motor_mode(PMD_Control_t * pmd, uint8_t value);
void set_motor_command(PMD_Control_t * pmd, int16_t value);
void get_motor_command(PMD_Control_t * pmd);
void get_motor_mode(PMD_Control_t * pmd);
void get_position( PMD_Control_t * pmd);
void get_current_A(PMD_Control_t * pmd);
void get_current_B(PMD_Control_t * pmd);
void get_velocity(PMD_Control_t * pmd);
uint8_t generate_checksum(volatile uint8_t * string, volatile uint8_t length);
void pmd_in_handle(PMD_Control_t * pmd);
void pmd_init(PMD_Control_t * pmd, USART_t * uart, int baud);
void pmd_reset(PMD_Control_t * pmd);
void set_commutation_mode(PMD_Control_t * pmd, uint8_t value);
void set_signal_sense(PMD_Control_t * pmd, uint16_t value);
void get_commutation_mode(PMD_Control_t * pmd);
void set_phase_counts(PMD_Control_t * pmd, uint16_t value);
void get_phase_counts(PMD_Control_t * pmd);
void set_dead_time(PMD_Control_t * pmd, int16_t value);
void get_dead_time(PMD_Control_t * pmd);
void set_pwm_limit(PMD_Control_t * pmd, int16_t value);
void get_pwm_limit(PMD_Control_t * pmd);
void get_activity_status(PMD_Control_t * pmd);
void set_condition_mask(PMD_Control_t * pmd, uint16_t value, uint16_t value1);
void set_loop_mode(PMD_Control_t * pmd, uint8_t value);
void get_loop_mode(PMD_Control_t * pmd);

void get_bus_voltage(PMD_Control_t * pmd);

//void set_loop_mode(PMD_Control_t * pmd, uint8_t loop_mode);
//uint8_t get_loop_mode(PMD_Control_t * pmd);
void set_velocity_feedback_source(PMD_Control_t * pmd, uint8_t vfb_source);
void get_velocity_feedback_source(PMD_Control_t * pmd);
void set_velocity_scalar(PMD_Control_t * pmd, uint16_t velocity_scalar);
void get_velocity_scalar(PMD_Control_t * pmd);
void set_velocity(PMD_Control_t * pmd, int32_t velocity);
void get_velocity(PMD_Control_t * pmd);
void set_acceleration(PMD_Control_t * pmd, uint32_t acceleration);
void get_acceleration(PMD_Control_t * pmd);
void set_sample_time(PMD_Control_t * pmd, uint16_t sample_time);
void get_sample_time(PMD_Control_t * pmd);
void set_loop_gain(PMD_Control_t * pmd, uint8_t loop_term, uint32_t value);
void get_loop_gain(PMD_Control_t * pmd, uint8_t loop_term);
void get_loop_error(PMD_Control_t * pmd, uint8_t loop_id);
void get_loop_integral(PMD_Control_t * pmd, uint8_t loop_id);
void set_motion_error_limit(PMD_Control_t * pmd, uint32_t limit);
void get_motion_error_limit(PMD_Control_t * pmd);
void set_phase_prescale(PMD_Control_t * pmd, uint8_t prescale_on);
void get_phase_prescale(PMD_Control_t * pmd);
void set_phase_correction_mode(PMD_Control_t * pmd, uint8_t correction_mode);
void get_phase_correction_mode(PMD_Control_t * pmd);
void set_phase_angle(PMD_Control_t * pmd, uint16_t phase);
void get_phase_angle(PMD_Control_t * pmd);
void set_command_source(PMD_Control_t * pmd, uint8_t cmd_source);
void get_command_source(PMD_Control_t * pmd);
void set_bus_voltage_limits(PMD_Control_t * pmd, uint8_t limit_type, uint16_t limit_value);
void get_bus_voltage_limits(PMD_Control_t * pmd, uint8_t limit_type);
void get_analog_offset(PMD_Control_t * pmd, uint8_t port_id);
void set_actual_position(PMD_Control_t * pmd, int32_t position);
void get_actual_position(PMD_Control_t * pmd);
void reset_event_status(PMD_Control_t * pmd, uint8_t reset_event_mask);
void get_signal_status(PMD_Control_t  * pmd);
void clear_position_error(PMD_Control_t * pmd);
void store_user_data(PMD_Control_t * pmd);
void pmd_set_baud(PMD_Control_t * pmd, int baud);
void set_phase_correction(PMD_Control_t * pmd, uint16_t value);
void get_loop_gain(PMD_Control_t * pmd, uint8_t value);
void get_loop_error(PMD_Control_t * pmd, uint8_t value);
void set_error_limit(PMD_Control_t * pmd, uint16_t value);
void get_event_status(PMD_Control_t * pmd);
void set_analog_offset(PMD_Control_t * pmd, uint8_t target,uint16_t value);








	
